﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/FrictionJoint.h
// Revision   : r29
// Change-Date: 2009-11-28
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/FrictionJoint.cpp
// Revision   : r31
// Change-Date: 2009-12-31
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Weld joint definition. You need to specify local anchor points 
    /// where they are attached and the relative body angle. The position 
    /// of the anchor points is important for computing the reaction torque.
    /// </summary>
    public class FrictionJointDef : JointDef
    {
        public FrictionJointDef()
        {
            type = JointType.FRICTION;
            localAnchorA = Vector2.Zero;
            localAnchorB = Vector2.Zero;
            maxForce = 0.0f;
            maxTorque = 0.0f;
        }

        /// <summary>Initialize the bodies, anchors, axis, and reference angle using the world
        /// anchor and world axis.</summary>
        public void Initialize(Body bA, Body bB, Vector2 anchor)
        {
            body1 = bA;
            body2 = bB;
            localAnchorA = body1.GetLocalPoint(anchor);
            localAnchorB = body2.GetLocalPoint(anchor);
        }

        /// <summary>The local anchor point relative to bodyA's origin.</summary>
        public Vector2 localAnchorA;

        /// <summary>The local anchor point relative to bodyB's origin.</summary>
        public Vector2 localAnchorB;

        /// <summary>The maximum friction force in N.</summary>
        public float maxForce;

        /// <summary>The maximum friction torque in N-m.</summary>
        public float maxTorque;
    };

    /// <summary>
    /// Friction joint. This is used for top-down friction.
    /// It provides 2D translational friction and angular friction.
    /// </summary>
    public class FrictionJoint : Joint
    {
        public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchorA);
        }

        public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchorB);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
            return inv_dt * m_linearImpulse;
        }

        public override float GetReactionTorque(float inv_dt)
        {
            return inv_dt * m_angularImpulse;
        }

        public void SetMaxForce(float force)
        {
            System.Diagnostics.Debug.Assert(Math.IsValid(force) && force >= 0.0f);
            m_maxForce = force;
        }

        public float GetMaxForce()
        {
            return m_maxForce;
        }

        public void SetMaxTorque(float torque)
        {
            System.Diagnostics.Debug.Assert(Math.IsValid(torque) && torque >= 0.0f);
            m_maxTorque = torque;
        }

        public float GetMaxTorque()
        {
            return m_maxTorque;
        }

        public FrictionJoint(FrictionJointDef def)
            : base(def)
        {
            m_localAnchorA = def.localAnchorA;
            m_localAnchorB = def.localAnchorB;

            m_linearImpulse = Vector2.Zero;
            m_angularImpulse = 0.0f;

            m_maxForce = def.maxForce;
            m_maxTorque = def.maxTorque;
        }

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body bA = m_body1; 
            Body bB = m_body2; 

            // Compute the effective mass matrix. 
            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter); 
            Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter); 

            // J = [-I -r1_skew I r2_skew] 
            //     [ 0       -1 0       1] 
            // r_skew = [-ry; rx] 

            // Matlab 
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB] 
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB] 
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB] 

            float mA = bA.m_invMass, mB = bB.m_invMass; 
            float iA = bA.m_invI, iB = bB.m_invI; 

            Mat22 K1 = new Mat22(); 
            K1.col1.X = mA + mB;    K1.col2.X = 0.0f; 
            K1.col1.Y = 0.0f;               K1.col2.Y = mA + mB; 

            Mat22 K2 = new Mat22(); 
            K2.col1.X =  iA * rA.Y * rA.Y;  K2.col2.X = -iA * rA.X * rA.Y; 
            K2.col1.Y = -iA * rA.X * rA.Y;  K2.col2.Y =  iA * rA.X * rA.X; 

            Mat22 K3 = new Mat22(); 
            K3.col1.X =  iB * rB.Y * rB.Y;  K3.col2.X = -iB * rB.X * rB.Y; 
            K3.col1.Y = -iB * rB.X * rB.Y;  K3.col2.Y =  iB * rB.X * rB.X; 

            Mat22 K = K1 + K2 + K3; 
            m_linearMass = K.GetInverse(); 

            m_angularMass = iA + iB; 
            if (m_angularMass > 0.0f) 
            { 
                 m_angularMass = 1.0f / m_angularMass; 
            } 

            if (step.warmStarting) 
            { 
                 // Scale impulses to support a variable time step. 
                 m_linearImpulse *= step.dtRatio; 
                 m_angularImpulse *= step.dtRatio; 

                 Vector2 P = new Vector2(m_linearImpulse.X, m_linearImpulse.Y); 

                 bA.m_linearVelocity -= mA * P; 
                 bA.m_angularVelocity -= iA * (Vector2Extensions.Cross(rA, P) + m_angularImpulse); 

                 bB.m_linearVelocity += mB * P; 
                 bB.m_angularVelocity += iB * (Vector2Extensions.Cross(rB, P) + m_angularImpulse); 
            } 
            else 
            { 
                 m_linearImpulse = Vector2.Zero; 
                 m_angularImpulse = 0.0f; 
            } 
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body bA = m_body1;
            Body bB = m_body2;

            Vector2 vA = bA.m_linearVelocity;
            float wA = bA.m_angularVelocity;
            Vector2 vB = bB.m_linearVelocity;
            float wB = bB.m_angularVelocity;

            float mA = bA.m_invMass, mB = bB.m_invMass;
            float iA = bA.m_invI, iB = bB.m_invI;

            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter);
            Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter);

            // Solve angular friction 
            {
                float Cdot = wB - wA;
                float impulse = -m_angularMass * Cdot;

                float oldImpulse = m_angularImpulse;
                float maxImpulse = step.dt * m_maxTorque;
                m_angularImpulse = MathHelper.Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = m_angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction 
            {
                Vector2 Cdot = vB + Vector2Extensions.Cross(wB, rB) - vA - Vector2Extensions.Cross(wA, rA);

                Vector2 impulse = -Math.Mul(m_linearMass, Cdot);
                Vector2 oldImpulse = m_linearImpulse;
                m_linearImpulse += impulse;

                float maxImpulse = step.dt * m_maxForce;

                if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    m_linearImpulse.Normalize();
                    m_linearImpulse *= maxImpulse;
                }

                impulse = m_linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * Vector2Extensions.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * Vector2Extensions.Cross(rB, impulse);
            }

            bA.m_linearVelocity = vA;
            bA.m_angularVelocity = wA;
            bB.m_linearVelocity = vB;
            bB.m_angularVelocity = wB;
        }
        
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            //B2_NOT_USED(baumgarte); 

            return true;
        }

        internal Vector2 m_localAnchorA;
        internal Vector2 m_localAnchorB;

        internal Mat22 m_linearMass;
        internal float m_angularMass;

        internal Vector2 m_linearImpulse;
        internal float m_angularImpulse;

        internal float m_maxForce;
        internal float m_maxTorque;
    }
}
